#include "Transformation.h"
//#include "./graph/core/graph_optimizer_sparse.cpp"
#include "./graph/core/hyper_graph.h"
#include "./graph/core/block_solver.h"
#include "./graph/core/solver.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/math_groups/se3quat.h"
#include "./graph/core/structure_only_solver.h"
//#include "./graph/vertex_se3_quat.cpp"
//#include "./graph/edge_se3_quat.cpp"

#include <string>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/passthrough.h>

#include <Eigen/Core>
#include <fstream>

void FrameNode::store(string path)
{
	printf("Framenode out file: %s\n",path.c_str());
	long size 				= sizeof(id)*(1+1+1+16);
	char * buffer_char 		= new char[size];
	int * buffer_int 		= (int *) buffer_char;
	float * buffer_float 	= (float *) buffer_char;
	buffer_int[0] 			= id;
	buffer_int[1] 			= locked;
	
	buffer_float[2] 		= matrix(0,0);
	buffer_float[3] 		= matrix(0,1);
	buffer_float[4] 		= matrix(0,2);
	buffer_float[5] 		= matrix(0,3);
	
	buffer_float[6] 		= matrix(1,0);
	buffer_float[7] 		= matrix(1,1);
	buffer_float[8] 		= matrix(1,2);
	buffer_float[9] 		= matrix(1,3);
	
	buffer_float[10]	 	= matrix(2,0);
	buffer_float[11]	 	= matrix(2,1);
	buffer_float[12]	 	= matrix(2,2);
	buffer_float[13]	 	= matrix(2,3);
	
	buffer_float[14]	 	= matrix(3,0);
	buffer_float[15]	 	= matrix(3,1);
	buffer_float[16]	 	= matrix(3,2);
	buffer_float[17]	 	= matrix(3,3);

	
	Transformation * current_trans = transformation;
	char buf [1024];
	
	sprintf (buf, "%s_frame", path.c_str());
	frame->store(string(buf));
	int i = 0;
	while(current_trans != 0)
	{
		sprintf (buf, "%s_transformation_%i", path.c_str(),i);
		current_trans->store(string(buf));
		current_trans = current_trans->next;
		i++;
	}
	
	buffer_float[18]	 	= i;
	ofstream outfile (path.c_str(),ofstream::binary);
	outfile.write (buffer_char,size);
	outfile.close();
}

void FrameNode::load(string path, RGBDCamera * cam,FrameNode ** nodes)
{
	printf("FrameNode load file: %s\n",path.c_str());
	printf("works\n");
	
	ifstream infile (path.c_str(),ifstream::binary);
	infile.seekg(0,ifstream::end);
	long size=infile.tellg();
	char * buffer_char 		= new char [size];
	int * buffer_int 		= (int *) buffer_char;
	float * buffer_float	= (float *) buffer_char;
	infile.seekg(0);
	infile.read (buffer_char,size);
	infile.close();
	
	id 			= buffer_int[0];			
	locked 		= buffer_int[1];
	matrix 		= Eigen::Matrix4f::Identity();	
	matrix(0,0) = buffer_float[2];
	matrix(0,1) = buffer_float[3];
	matrix(0,2) = buffer_float[4];
	matrix(0,3) = buffer_float[5];
	matrix(1,0) = buffer_float[6];
	matrix(1,1) = buffer_float[7];
	matrix(1,2) = buffer_float[8];
	matrix(1,3) = buffer_float[9];
	matrix(2,0) = buffer_float[10];
	matrix(2,1) = buffer_float[11];
	matrix(2,2) = buffer_float[12];
	matrix(2,3) = buffer_float[13];
	matrix(3,0) = buffer_float[14];
	matrix(3,1) = buffer_float[15];
	matrix(3,2) = buffer_float[16];
	matrix(3,3) = buffer_float[17];
	int nr_trans= buffer_float[18];
	delete buffer_char;
	char buf [1024];
	RGBDFrame * current_frame = new RGBDFrame();
	sprintf (buf, "%s_frame", path.c_str());
	current_frame->load(string(buf),cam);
	frame = current_frame;
	Transformation * prev = 0;
	for(int i = 0; i < nr_trans; i++)
	{
		sprintf (buf, "%s_transformation_%i", path.c_str(),i);
		Transformation * current = new Transformation();
		current->load(string(buf),nodes);
		if(i == 0)
		{
			transformation = current;
			prev = current;
		}
		else
		{
			prev->next = current;
			current->prev = prev;
			prev = current;
		}
	}
}
